/// @file Boid.cpp
/// @brief the unit boid of the flocking system
/// implements the flocking rules for a boid

#include "boost/foreach.hpp"

#include "Boid.h"
#include "Utilities.h"
#include "Configuration.h"


Boid::Boid
            (
                const int _id,
                const ngl::Colour _col,
                const ngl::Vector _pos,
                const ngl::Vector _vel
            ) :
            MovingObject(_pos, _vel)
{
    //init params
    m_id = _id;
    m_col.set(_col);

    //create list of neighbours
    m_neighbourList = new std::vector<Boid*>;

    std::cout << "Boid created" << std::endl;
}

Boid::~Boid()
{
    //delete neighbour list
    delete m_neighbourList;

    std::cout << "Boid deleted" << std::endl;
}

void Boid::planNextMove
        (
            const bool _normaliseRules,
            const float _separationDistance,
            const bool _enableCohesion,
            const bool _enableSeparation,
            const bool _enableAlignment,
            const float _cohesionWeight,
            const float _separationWeight,
            const float _alignmentWeight,
            const bool _enableObstacleAvoidance,
            const float _obstacleAvoidanceWeight,
            const bool _enableTendToward,
            const ngl::Vector _tendPosition,
            const float _tendTowardWeight,
            const bool _enableExternalForce,
            const ngl::Vector _externalForce,
            const float _externalForceWeight,
            const float _minSpeed,
            const float _maxSpeed,
            const float _boidScaleFactor,
            ObstacleManager* io_obstacleManager,
            const bool _enable2DTopView
        )
{
    //synchronise boid's bounding sphere radius
    m_boundingRadius = _boidScaleFactor;

    //check collision against obstacles
    bool isVelocityReversed = false;
    ngl::Vector inc_collisionAvoidance = 0;
    if (_enableObstacleAvoidance && io_obstacleManager->getEnableObstacle()) inc_collisionAvoidance = applyObstacleAvoidance(_obstacleAvoidanceWeight, _normaliseRules, io_obstacleManager, isVelocityReversed, _enable2DTopView);

    if (isVelocityReversed)
    {
        //collision ahead -> engage absolute obstacle avoidance => reverseVelocity
        reverseVelocity();

        if (Configuration::s_debug) std::cout << "BOID : plan next move - absolute collision avoidance engaged -> vel reversed to : " << m_velocity << std::endl;
    }
    else
    {
        //initialise rules velocity
        ngl::Vector inc_cohesion = 0;
        ngl::Vector inc_separation = 0;
        ngl::Vector inc_alignment = 0;
        ngl::Vector inc_tendToward = 0;
        ngl::Vector inc_externalForce = 0;

        //get velocities from various rules
        if (_enableCohesion) inc_cohesion = applyCohesion(_cohesionWeight, _normaliseRules);
        if (_enableSeparation) inc_separation = applySeparation(_separationWeight, _normaliseRules, _separationDistance);
        if (_enableAlignment) inc_alignment = applyAlignment(_alignmentWeight, _normaliseRules);

        if (_enableTendToward) inc_tendToward = applyTendToward(_tendTowardWeight, _normaliseRules, _tendPosition);
        if (_enableExternalForce) inc_externalForce = applyExternalForce(_externalForceWeight, _normaliseRules, _externalForce);

        if (Configuration::s_debug) std::cout << "BOID : plan next move - post rules : [" << m_velocity.length() << ", " << inc_cohesion.length() << ", " << inc_separation.length() << ", " << inc_alignment.length() << ", " << inc_tendToward.length() << ", " << inc_externalForce.length() << ", " << inc_collisionAvoidance.length() << "]" << std::endl;

        //determine the emphasis on separation
        float separationEmphasisRatio = 1.0 - inc_separation.length();

        //then add the total velocities change to the velocity to get a new velocity
        m_velocity =  ((m_velocity + inc_cohesion + inc_alignment + inc_tendToward + inc_externalForce) * separationEmphasisRatio) + inc_separation + inc_collisionAvoidance;

        if (Configuration::s_debug) std::cout << "BOID : plan next move - post rules : finals : [" << m_velocity.length() * separationEmphasisRatio << ", " << inc_cohesion.length()* separationEmphasisRatio << ", " << inc_separation.length() << ", " << inc_alignment.length()* separationEmphasisRatio << ", " << inc_tendToward.length()* separationEmphasisRatio << ", " << inc_externalForce.length()* separationEmphasisRatio << ", " << inc_collisionAvoidance.length() << "]" << std::endl;

    }

    //limit velocity to a max speed
    applyVelocityLimit(_minSpeed, _maxSpeed);

}

void Boid::move()
{
    //move boid with normal velocity
    MovingObject::move(m_velocity);
}

void Boid::move(const ngl::Vector _velocity)
{
    //move boid with some given velocity
    MovingObject::move(_velocity);
}

ngl::Vector Boid::applyCohesion
                        (
                            const float _weight,
                            const bool _normalise
                        )
{
    if (m_neighbourList->size() == 0)
    {
        //no neighbour

        if (Configuration::s_debug) std::cout << "Boid : apply cohesion : ME : " << m_id << "\twith no neighbours" << std::endl;

        return 0;
    }
    else
    {
        if (Configuration::s_debug) std::cout << "Boid : apply cohesion : ME : " << m_id << "\tneighbour count : " << m_neighbourList->size() << "\tweight : " << _weight << std::endl;

        ngl::Vector target = 0;

        //get the average position of all neighbouring boids
        BOOST_FOREACH(Boid* boid, *m_neighbourList)
        {
            //calculate the sum of positions of neighbours
            target += boid->getPosition();
        }

        //calculate the average position
        target /= m_neighbourList->size();

        //get vector from boid's current position to the desired average position
        ngl::Vector targetVector = target - getPosition();

        //and determine a small increment to it -> controllable weight
        ngl::Vector weightedVector = _normalise ? Utilities::scaleVectorBy(targetVector, _weight) : targetVector * _weight;

        if (Configuration::s_debug) std::cout << "BOID : plan next move -> apply cohesion : V : " << targetVector << "\tdV : " << weightedVector << std::endl;

        return weightedVector;
    }
}

ngl::Vector Boid::applySeparation
                            (
                                const float _weight,
                                const bool _normalise,
                                const float _separationDistance
                            )
{
    if (m_neighbourList->size() == 0)
    {
        //no neighbour

        if (Configuration::s_debug) std::cout << "Boid : apply separation : ME : " << m_id << "\twith no neighbours" << std::endl;

        return 0;
    }
    else
    {
        if (Configuration::s_debug) std::cout << "Boid : apply separation : ME : " << m_id << "\tneighbour count : " << m_neighbourList->size() << "\tweight : " << _weight << std::endl;

        ngl::Vector targetVector = 0;

        //definite count as the number of neighbours within a close distance
        int count = 0;
        ngl::Vector distanceVector;

        //get the average distance of all neighbouring boids
        BOOST_FOREACH(Boid* boid, *m_neighbourList)
        {
            //get distance from center boid to neighbour
            distanceVector = boid->getPosition() - getPosition();

            if (Configuration::s_debug) std::cout << "BOID - in sepration : distance from me : " << m_id << "\tto neighbour : " << boid->getId() << "\t= : " << distanceVector.length() << std::endl;

            if (distanceVector.length() < _separationDistance)
            {
                //neighbour boid is within a close distance

                //calculate the sum of positions of neighbours from boid's
                targetVector -= boid->getPosition() - getPosition();

                //update count
                count++;
            }
        }

        if (count == 0)
        {
            //no neighbour is within consideration distance -> no need to apply separation

            if (Configuration::s_debug) std::cout << "Boid : apply separation : ME : " << m_id << "\twith no neighbours within closedistance" << std::endl;

            return 0;
        }
        else
        {
            //calculate the average distance
            targetVector /= count;

            //and determine a small increment to it -> controllable weight
            ngl::Vector weightedVector = _normalise ? Utilities::scaleVectorBy(targetVector, _weight) : targetVector * _weight;

            if (Configuration::s_debug) std::cout << "BOID : plan next move -> apply sepration : V : " << targetVector << "\tdV : " << weightedVector << std::endl;

            return weightedVector;
        }
    }
}

ngl::Vector Boid::applyAlignment
                            (
                                const float _weight,
                                const bool _normalise
                            )
{
    if (m_neighbourList->size() == 0)
    {
        //no neighbour

        if (Configuration::s_debug) std::cout << "Boid : apply separation : ME : " << m_id << "\twith no neighbours" << std::endl;

        return 0;
    }
    else
    {
        if (Configuration::s_debug) std::cout << "Boid : apply alignment : ME : " << m_id << "\tneighbour count : " << m_neighbourList->size() << "\tweight : " << _weight << std::endl;

        ngl::Vector targetVector = 0;

        //get the average velocities of all neighbouring boids
        BOOST_FOREACH(Boid* boid, *m_neighbourList)
        {
            //calculate the sum of velocities of neighbours
            targetVector += boid->getVelocity();
        }

        //calculate the average velocity
        targetVector /= m_neighbourList->size();

        //and determine a small increment to it -> controllable weight
        ngl::Vector weightedVector = _normalise ? Utilities::scaleVectorBy(targetVector, _weight) : targetVector * _weight;

        if (Configuration::s_debug) std::cout << "BOID : plan next move -> apply alignment : V : " << targetVector << "\tdV : " << weightedVector << std::endl;

        return weightedVector;
    }
}

ngl::Vector Boid::applyTendToward
                        (
                            const float _weight,
                            const bool _normalise,
                            const ngl::Vector _tendPosition
                        )
{
    if (Configuration::s_debug) std::cout << "Boid : apply tend toward : ME : " << m_id << "\tweight : " << _weight << std::endl;

    //get vector from boid's current position to the desired position
    ngl::Vector targetVector = _tendPosition - getPosition();

    //and determine a small increment to it -> controllable weight
    ngl::Vector weightedVector = _normalise ? Utilities::scaleVectorBy(targetVector, _weight) : targetVector * _weight;

    if (Configuration::s_debug) std::cout << "BOID : plan next move -> apply tend toward : V : " << targetVector << "\tdV : " << weightedVector << std::endl;

    return weightedVector;
}

ngl::Vector Boid::applyExternalForce
                            (
                                const float _weight,
                                const bool _normalise,
                                const ngl::Vector _externalForce
                            )
{
    if (Configuration::s_debug) std::cout << "Boid : apply external force : ME : " << m_id << "\tweight : " << _weight << std::endl;

    //set the external force vector
    ngl::Vector targetVector = _externalForce;

    //and determine a small increment to it -> controllable weight
    ngl::Vector weightedVector = _normalise ? Utilities::scaleVectorBy(targetVector, _weight) : targetVector * _weight;

    if (Configuration::s_debug) std::cout << "BOID : plan next move -> apply external force : V : " << targetVector << "\tdV : " << weightedVector << std::endl;

    return weightedVector;
}

ngl::Vector Boid::applyObstacleAvoidance
                                (
                                    const float _weight,
                                    const bool _normalise,
                                    ObstacleManager* io_obstacleManager,
                                    bool& o_isToBeReversed,
                                    const bool _enable2DTopView
                                )
{
    if (Configuration::s_debug) std::cout << "Boid : apply obstacle avoidance : ME : " << m_id << std::endl;

    ngl::Vector avoidanceVector = 0;

    //for all the obstacles, check if boid is inside their spheres
    BOOST_FOREACH(StaticObject* obstacle, *io_obstacleManager->getObstacleList())
    {

        //check if boid is inside the bounding sphere
        bool insideBoundingSphere = Utilities::isObjectInsideSphere(obstacle->getPosition(), obstacle->getBoundingRadius(), getPosition(), m_boundingRadius, getVelocity());
        if (insideBoundingSphere)
        {
            //bounding sphere is breached -> serious case

            //update obstacle stats
            io_obstacleManager->incrementBoundingSphereHit();

            if (Configuration::s_debug) std::cout << "Boid : inside bounding sphere -> reverse mmediately" << std::endl;

            //serious case -> must reverse
            o_isToBeReversed = true;

            //return 0 -> not required since velocity will be reversed
            return 0;
        }

        //check if boid is inside the influence sphere
        bool insideInfluenceSphere = Utilities::isObjectInsideSphere(obstacle->getPosition(), obstacle->getInfluenceRadius(), getPosition(), m_boundingRadius, getVelocity());
        if (insideInfluenceSphere)
        {
            //inside influence sphere -> not so serious but needs to act now

            //update obstacle stats
            io_obstacleManager->incrementInfluenceSphereHit();

            //find workaround velocity to steer boid away from obstacle
            avoidanceVector = Utilities::getWorkaroundVelocity(obstacle->getPosition(), obstacle->getInfluenceRadius(), getPosition(), _enable2DTopView);

            if (Configuration::s_debug) std::cout << "Boid : inside influence sphere -> find workaround vel : " << avoidanceVector << std::endl;

            //add weight to it
            avoidanceVector = _normalise ? Utilities::scaleVectorBy(avoidanceVector, _weight) : avoidanceVector * _weight;

            return avoidanceVector;
        }
    }

    //else, no collision => no collision avoidance
    return 0;
}

void Boid::applyVelocityLimit
                            (
                                const float _minSpeed,
                                const float _maxSpeed
                            )
{
    //limit velocity to a maximum value
    if (m_velocity.length() > _maxSpeed)
    {
        if (Configuration::s_debug) std::cout << "BOID - limit velocity : current : " << m_velocity.length();

        //normalise and scale vector to maxSpeed
        m_velocity.set(Utilities::scaleVectorBy(m_velocity, _maxSpeed));

        if (Configuration::s_debug) std::cout << "\tlimited : " << m_velocity.length() << std::endl;
    }

    //limit velocity to a minimum value
    if (m_velocity.length() < _minSpeed)
    {
        if (Configuration::s_debug) std::cout << "BOID - uplimit velocity : current : " << m_velocity.length();

        //normalise and scale vector to minSpeed
        m_velocity.set(Utilities::scaleVectorBy(m_velocity, _minSpeed));

        if (Configuration::s_debug) std::cout << "\tuplimited : " << m_velocity.length() << std::endl;
    }
}

void Boid::addNeighbour(Boid* io_neighbour)
{
    //add the boid to list of neighbours
    m_neighbourList->push_back(io_neighbour);

    if (Configuration::s_debug) std::cout << "BOID " << getId() << "\t NEIGHBOUR ADDED : " << io_neighbour->getId() << std::endl;
}

void Boid::clearNeighbours()
{
    //clear list of neighbours
    m_neighbourList->clear();
}
